#include "drv_dji.h"

#include "stdlib.h"

/* 电流指令缓冲区 */
uint8_t motorCmdBuf[2][8] = {0};

/**
 * @brief 初始化一个大疆电机实例
 * @param motor: 大疆电机实例
 * @param motorInit: 大疆电机初始化结构体
 * @retval None
 */
void DJI_Init(Dji_t *motor, DjiInit_t *motorInit)
{
    motor->motorID = motorInit->motorID;
    motor->motorIDGroup = (motor->motorID <= 4) ? 0 : 1;

    motor->hcan = motorInit->hcan;
    motor->cmdStdID = 0x200 - motor->motorIDGroup;
    motor->cmdBufRow = motor->motorIDGroup;
    motor->cmdBufColHigh = 2 * ((motor->motorID + motor->motorIDGroup) % 5) - 2;
    motor->cmdBufColLow = 2 * ((motor->motorID + motor->motorIDGroup) % 5) - 1;

    motor->initFlag = DISABLE;
    motor->initCnt = 0;
    motor->lastAngle = 0;
    motor->overflowCnt = 0;
}

/**
 * @brief 处理大疆电机实例的电流指令
 * @param motor: 大疆电机实例
 * @param iq: 电流值
 * @param mutex: 互斥锁
 * @retval None
 */
void DJI_WriteCmdBuf(Dji_t *motor, int16_t iq, SemaphoreHandle_t mutex)
{
    if (xSemaphoreTake(mutex, 0) != pdTRUE)
    {
        return;
    }

    CAN_TxHeaderTypeDef txHeader = {0};
    txHeader.StdId = motor->cmdStdID;
    txHeader.RTR = CAN_RTR_DATA;
    txHeader.IDE = CAN_ID_STD;
    txHeader.DLC = 8;
    txHeader.TransmitGlobalTime = DISABLE;

    motorCmdBuf[motor->cmdBufRow][motor->cmdBufColHigh] = iq >> 8;
    motorCmdBuf[motor->cmdBufRow][motor->cmdBufColLow] = iq & 0xFF;
    if (HAL_CAN_GetTxMailboxesFreeLevel(motor->hcan) > 0)
    {
        HAL_CAN_AddTxMessage(motor->hcan, &txHeader, motorCmdBuf[motor->cmdBufRow] + 0, NULL);
    }

    xSemaphoreGive(mutex);
}

/**
 * @brief 此函数放置在ISR中, 更新大疆电机实例的反馈信息
 * @param rxData: CAN接收的缓存数组
 * @param motor: 大疆电机实例
 * @param motorInfoBoxHandle: 存放电机反馈信息的邮箱句柄
 * @retval None
 */
void ISR_DJI_UpdateInfo(uint8_t rxData[], Dji_t *motor, QueueHandle_t motorInfoBoxHandle)
{
    if (motor->initFlag == DISABLE && ++(motor->initCnt) >= 32)
    {
        motor->lastAngle = (rxData[0] << 8) | rxData[1];
        motor->initFlag = ENABLE;
    }
    else if (motor->initFlag == ENABLE)
    {
        DjiMsg_t infoMsg = {0};

        infoMsg.angle = (rxData[0] << 8) | rxData[1];
        infoMsg.speed = (rxData[2] << 8) | rxData[3];
        infoMsg.torque = (rxData[4] << 8) | rxData[5];

        const int16_t temp = (int16_t)infoMsg.angle - (int16_t)motor->lastAngle;
        if (abs(temp) > 4000)
        {
            temp < 0 ? (motor->overflowCnt)++ : (motor->overflowCnt)--;
        }
        infoMsg.overflowCnt = motor->overflowCnt;

        motor->lastAngle = infoMsg.angle;

        xQueueOverwriteFromISR(motorInfoBoxHandle, &infoMsg, NULL);
    }
}

/**
 * @brief 获取2006电机输出端转速, 单位为rpm
 */
float DJI_GetM2006Rpm(QueueHandle_t motorInfoBoxHandle)
{
    DjiMsg_t msg;
    xQueuePeek(motorInfoBoxHandle, &msg, portMAX_DELAY);

    return (float)msg.speed * (1.0 / 36.0);
}

/**
 * @brief 获取3508电机输出端转速, 单位为rpm
 */
float DJI_GetM3508Rpm(QueueHandle_t motorInfoBoxHandle)
{
    DjiMsg_t msg;
    xQueuePeek(motorInfoBoxHandle, &msg, portMAX_DELAY);

    return (float)msg.speed * (187.0 / 3591.0);
}

/**
 * @brief 获取大疆电机的编码器计数
 */
int32_t DJI_GetCnt(QueueHandle_t motorInfoBoxHandle)
{
    DjiMsg_t msg;
    xQueuePeek(motorInfoBoxHandle, &msg, portMAX_DELAY);

    return (int32_t)msg.angle + (int32_t)msg.overflowCnt * 8192;
}

/**
 * @brief 获取2006电机输出端位置, 单位为r
 */
float DJI_GetM2006Pos(QueueHandle_t motorInfoBoxHandle, int32_t baseCnt)
{
    return (float)(DJI_GetCnt(motorInfoBoxHandle) - baseCnt) / 8192.0 * (1.0 / 36.0);
}

/**
 * @brief 获取3508电机输出端位置, 单位为r
 */
float DJI_GetM3508Pos(QueueHandle_t motorInfoBoxHandle, int32_t baseCnt)
{
    return (float)(DJI_GetCnt(motorInfoBoxHandle) - baseCnt) / 8192.0 * (187.0 / 3591.0);
}
